import threading
import rospy
from sensor_msgs.msg import Image
import cv2
import ros_numpy


class cam():

    def __init__(self):
        self.depthimg = None
        self.img = None
        self.depthFlag = False
        self.imgFlag = False
        rospy.init_node('depth_image_subscriber', anonymous=True)
        camgetimage = threading.Thread(target=self._cam)
        camgetimage.start()

        while self.imgFlag == False or self.depthFlag == False:
            cv2.waitKey(1)
        print("Camara Node Inited.")

    def _cam(self):
        rospy.Subscriber('/camera/aligned_depth_to_color/image_raw', Image, self.depth_image_callback)
        rospy.Subscriber('/camera/color/image_raw',Image,self.image_callback)
        rospy.spin()

    def depth_image_callback(self,msg):
        image = ros_numpy.numpify(msg)
        self.depthimg = image
        self.depthFlag = True

    def image_callback(self,msg):
        image = ros_numpy.numpify(msg)
        image = cv2.cvtColor(image,cv2.COLOR_BGR2RGB)
        self.img = image
        self.imgFlag = True
        
    def getimage(self):
        return self.img

    def getdepthimage(self):
        return self.depthimg
        
    def getdistance(self,point):
        x,y = point
        return self.depthimg[y,x]


def rotation_matrix_to_euler_angles(R):
    import numpy as np
    sy = np.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])

    singular = sy < 1e-6

    if not singular:
        x = np.arctan2(R[2, 1], R[2, 2])
        y = np.arctan2(-R[2, 0], sy)
        z = np.arctan2(R[1, 0], R[0, 0])
    else:
        x = np.arctan2(-R[1, 2], R[1, 1])
        y = np.arctan2(-R[2, 0], sy)
        z = 0

    return np.degrees(x), np.degrees(y), np.degrees(z)

if __name__ == "__main__":
    from ATag import *
    a = cam()
    ar = AprilTag()

    while 1:
        img = a.getimage()
        depth_img = a.getdepthimage()

         

        center,img = ar.detect(img)
        print(center) #[[509.5, 267.5]]
        # print(ar.returnListFourPoint)

        # if len(ar.returnListFourPoint) == 0 :
        #     continue

        # rvec,tvec = ar.PNP(160,ar.returnListFourPoint[0])

        # rmtx, _ = cv2.Rodrigues(rvec)

        # # print(rmtx)
        # print("pitch yaw roll")
        # print(rotation_matrix_to_euler_angles(rmtx))
            
        # # print(a.getdistance((600,600)))
        # cv2.imwrite("1.jpg",img)

        cv2.imshow("",img)
        cv2.imshow("1",depth_img)
        cv2.waitKey(1)
    


# if __name__ == "__main__":

#     a = cam()

#     while 1:
#         img = a.getimage()
#         depth_img = a.getdepthimage()
        
#         print(a.getdistance((600,600)))

#         cv2.imshow("",img)
#         cv2.imshow("1",depth_img)
#         cv2.waitKey(1)
    